/**************************************************************************
 * @file usr_pid.c
 * @author 黑马小乌龟 (532352406@qq.com)
 * @brief 
 * @version 0.1
 * @date 2025-08-13
 * 
 * @copyright Copyright (c) 2025
 * 
 *************************************************************************/
#include "usr_cfg.h"
#include "usr_pid.h"
#include <stdio.h>
#include <stdint.h>

// 初始化控制器
void usr_PID_Init(usr_IncPID *pid, float Kp, float Ki,/*float Ti, float Ts, */
              float min, float max, float inc_max) {
    pid->Kp = Kp;
    // pid->Ki = (Ti > 1e-6f) ? (Kp * Ts / Ti) : 0.0f;  // 防除零
    pid->Ki = Ki;
    pid->out_min = min;
    pid->out_max = max;
    pid->inc_max = inc_max;
    pid->prev_error = 0.0f;
    pid->prev_output = 0.0f;
    int tt = (pid->Ki*10000);
    DDL_Printf("ki:%d\n",tt);
}

// 最推荐的纯C优化版本 - 性能优秀且可维护
__attribute__((always_inline, optimize("O3")))
static inline float fast_flimit(float value, float min_val, float max_val) {
    return (value > max_val) ? max_val : ((value < min_val) ? min_val : value);
}
// 执行控制计算
// 在函数定义前添加section属性
RAM_FUNC float usr_PID_Compute(usr_IncPID *p_pid, float setpoint, float pv) {
    usr_IncPID *pid = p_pid;
    /*1. 计算当前误差*/
    float error = setpoint - pv;

    /*2. 计算比例项增量*/
    float p_term = pid->Kp * (error - pid->prev_error);

    /*3. 计算积分项增量*/
    float i_term = pid->Ki * error;

    /*4. 计算总增量*/
    float delta = p_term + i_term;

    /*5. 应用增量限制*/
    if (pid->inc_max > 0) {
        if (delta > pid->inc_max) delta = pid->inc_max;
        if (delta < -pid->inc_max) delta = -pid->inc_max;
    }
    /*6. 计算新输出*/
    float output = delta + p_term;
    /*7. 应用输出限幅*/
    if (output > pid->out_max) output = pid->out_max;
    if (output < pid->out_min) output = pid->out_min;
    /*8. 更新状态*/
    pid->prev_error = error;
    // pid->prev_output = output;
    return output;
}
// 重置控制器状态
RAM_FUNC void usr_PID_Reset(usr_IncPID *pid, float init_output) {
    pid->prev_error = 0.0f;
    pid->prev_output = init_output;  // 初始输出值
}






// 初始化控制器
void usr_pi_int_Init(usr_int_pi *pid, float Kp, float Ki, int32_t min, int32_t max, int32_t inc_max) {
    pid->Kp = Kp;
    // pid->Ki = (Ti > 1e-6f) ? (Kp * Ts / Ti) : 0.0f;  // 防除零
    pid->Ki = Ki;
    pid->out_min = min;
    pid->out_max = max;
    pid->inc_max = inc_max;
    pid->prev_error = 0;
    pid->prev_output = 0;
    // int tt = (pid->Ki*10000);
    // DDL_Printf("ki:%d\n",tt);
}

// 执行控制计算
// 在函数定义前添加section属性
RAM_FUNC int32_t usr_pi_int_compute(usr_int_pi *p_pid, int32_t setpoint, int32_t pv) {
    usr_int_pi *pid = p_pid;
    /*1. 计算当前误差*/
    float error = setpoint - pv;

    /*2. 计算比例项增量*/
    float p_term = pid->Kp * (error - pid->prev_error);

    /*3. 计算积分项增量*/
    float i_term = pid->Ki * error;

    /*4. 计算总增量*/
    float delta = p_term + i_term;

    /*5. 应用增量限制*/
    if (pid->inc_max > 0) {
        if (delta > pid->inc_max) delta = pid->inc_max;
        if (delta < -pid->inc_max) delta = -pid->inc_max;
    }
    /*6. 计算新输出*/
    int32_t output = delta + p_term;
    /*7. 应用输出限幅*/
    if (output > pid->out_max) output = pid->out_max;
    if (output < pid->out_min) output = pid->out_min;
    /*8. 更新状态*/
    pid->prev_error = error;
    // pid->prev_output = output;
    return output;
}

// 重置控制器状态
RAM_FUNC void usr_pi_int_reset(usr_int_pi *pid, int32_t init_output) {
    pid->prev_error = 0;
    pid->prev_output = init_output;  // 初始输出值
}